/** Main file of the quadrocopter project.
 *
 * This file contains the main routine of the quadrocopter project. Furthermore,
 * global variables are defined here.
*/
//##############################################################################




/** Board specific defines etc from ASF */
#include "board.h"
//------------------------------------------------------------------------------




/** Compiler defines etc from ASF */
#include "compiler.h"
//------------------------------------------------------------------------------




/** Power management from ASF */
#include "pm.h"
//------------------------------------------------------------------------------




/** Interrupt control features from ASF */
#include "intc.h"
//------------------------------------------------------------------------------




/** Display interface from LR */
#include "LR_display.h"
//------------------------------------------------------------------------------




/** Ringbuffer library code from LR */
#include "LR_ringbuffer.h"
//------------------------------------------------------------------------------




/** USART interface from LR */
#include "LR_usart.h"
//------------------------------------------------------------------------------




/** TTY interface from LR */
#include "LR_tty.h"
//------------------------------------------------------------------------------




/** TWI master interface from LR */
#include "LR_twimaster.h"
//------------------------------------------------------------------------------




/** For use of timers */
#include "LR_timer.h"
//------------------------------------------------------------------------------




/** ITG and ADXL abstraction layers */
#include "itg.h"
#include "adxl.h"
//------------------------------------------------------------------------------




/** Calibration interface */
#include "calibration.h"
//------------------------------------------------------------------------------




/** Display control functions */
#include "display.h"
//------------------------------------------------------------------------------




/** Main logic for controlling the system */
#include "control.h"
//------------------------------------------------------------------------------




/** Global variables */
#include "globals.h"
//##############################################################################




/** Ringbuffer to store received chars from USART */
LR_ringbuffer rb_usart;
//------------------------------------------------------------------------------




/** Size of the ringbuffer */
#define __rb_usart_buffersize 256
//------------------------------------------------------------------------------




/** Actual buffer of the ringbuffer */
int __rb_usart_buffer[__rb_usart_buffersize];
//------------------------------------------------------------------------------




/** Ringbuffer to store button and joystick events */
LR_ringbuffer rb_tty;
//------------------------------------------------------------------------------




/** Size of the ringbuffer */
#define __rb_tty_buffersize 32
//------------------------------------------------------------------------------




/** Actual buffer of the ringbuffer */
int __rb_tty_buffer[__rb_tty_buffersize];
//##############################################################################


#include <stdio.h>

/** Main function */
int main( void )
{
  unsigned char display_page;
  unsigned char cursor;
  int n;
  unsigned char step_iterator = 0;
  double step_size [] = {10.0, 1.0, 0.1, 0.01, 0.001, 0.0001};
  
  // Switch the main oscilator
  pm_switch_to_osc0( &AVR32_PM, FOSC0, OSC0_STARTUP );

  // Setup interrupts
  Disable_global_interrupt();
  INTC_init_interrupts();
  Enable_global_interrupt();

  // Enable the display interface
  LR_display_init( FOSC0 );

  // Enable the USART interface
  LR_ringbuffer_init( &rb_usart, __rb_usart_buffer, __rb_usart_buffersize );
  LR_usart_init( FOSC0, &rb_usart );

  // Enable the TTY interface
  LR_ringbuffer_init( &rb_tty, __rb_tty_buffer, __rb_tty_buffersize );
  LR_tty_init( &rb_tty );

  // Enable the TWI master interface
  LR_twimaster_init( FOSC0, 50000, 0x68 );

  // Enable sensors
  if (itg_init()) LR_display_write(0,0,"Init ok (1/2)");
  if (adxl_init()) LR_display_write(0,1,"Init ok (2/2)");

  // Init the timers
  LR_timer_init( 0, FOSC0 );
  LR_timer_init( 1, FOSC0 );
  LR_timer_init( 2, FOSC0 );

  // Calibrate sensors
  calibrate( 1 );

  // Set default values
  globals_set_default();

  // Set display timer
  //display_handler_start( 0, 500 );

  // Set control timer
  // TODO IMPORTANT TO USE TIMER 2 BECAUSE OF CALIBRATION
  control_handler_start( 1, 5 );
  LR_display_write(0,0,"hi");
  while (1 == 1)
  {
    /*static int mimimi = 0;
    if (mimimi < 100) mimimi++;
    else
    {
      static int mimi = 0;
      mimi++;
      if (mimi == 19)
        mimi = 0;
      LR_display_write(mimi,0," .");
      mimimi = 0;
    }*/
    // Get events from TTY (buttons or joystick)
    if (LR_ringbuffer_get( &rb_tty, &n ))
    {
      LR_usart_write(".");

      display_page = display_handler_pget();
      cursor = display_handler_cget();
      // Emergency stop button to stop the engines no matter what the rest of
      // the system is doing
      if (n == LR_TTY_PB0)
      {
        ctl_engine = 0;
      }
      // Move on to the previous page. Also resets the cursor
      else if (n == LR_TTY_PB1)
      {
        display_handler_pleft();
      }
      // Move on to the next page. Also resets the cursor.
      else if (n == LR_TTY_PB2)
      {
        display_handler_pright();
      }
      // Move the cursor one line up
      else if (n == LR_TTY_UP)
      {
        display_handler_cup();
      }
      // Move the cursor one line down
      else if (n == LR_TTY_DOWN)
      {
        display_handler_cdown();
      }
      // Decrease the selected value, if any
      else if (n == LR_TTY_LEFT)
      {
        // decrease
        if (display_page == 1)
        {
          pid_yaw[cursor] -= step_size[step_iterator];
        }
        else if (display_page == 2)
        {
          pid_attitude[cursor] -= step_size[step_iterator];
        }
      }
      // Increase the selected value, if any
      else if (n == LR_TTY_RIGHT)
      {
        // increase
        if (display_page == 1)
        {
          pid_yaw[cursor] += step_size[step_iterator];
        }
        else if (display_page == 2)
        {
          pid_attitude[cursor] += step_size[step_iterator];
        }
      }
      // Change the step size for values OR toggle a event
      else if (n == LR_TTY_PUSH)
      {
        // Adjust step size
        if ((display_page == 1) || (display_page == 2))
        {
          step_iterator++;
          if (step_iterator >= (sizeof( step_size ) / sizeof( step_size[0] )))
          {
            step_iterator = 0;
          }
        }
        else if (display_page == 6)
        {
          // toggle
          if (cursor == 0)
          {
            ctl_engine ^= 1;
          }
          else if (cursor == 1)
          {
            ctl_calibration = 1;
            calibrate( 1 );
            ctl_calibration = 0;
          }
          else if (cursor == 2)
          {
            ctl_quaternion ^= 1;
          }
        }
        else if (display_page == 7)
        {
          // toggle
          if (cursor == 0)
          {
            ctl_attitude ^= 1;
          }
          else if (cursor == 1)
          {
            ctl_yaw ^= 1;
          }
          else if (cursor == 2)
          {
            ctl_position ^= 1;
          }
        }
      }
    }

    display_handler_run();
  }

  // On termination
  while (1) {};
  
  // We will never arive here
  return 0;
}
//------------------------------------------------------------------------------




